// v1.0 07/07/13 created

#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/nonfree/features2d.hpp"     // SURF & SIFT headers have been moved here!!!!
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc/imgproc_c.h"

#include <iostream>
#include <vector>
#include <stdio.h>

#include "OpenCV/MatWork.hpp"
#include "FolderSparser.h"
#include "CoordTransf.hpp"

#include "PBVS.hpp"

using namespace std;
using namespace cv;

const int sample_sz = 400;   // 500 crashes the memory :))

void RodriguesTest()
{
	Mat R = Mat::eye(3,3,CV_64F);
	Mat r = Mat::zeros(3,1,CV_64F);

	double ROL, PIT, YAW;
	ROL = 0;
	PIT = 0;
	YAW = 0;

	Mat tmp = roty(CV_PI/2.0)*rotx(-CV_PI)*rotz(CV_PI)*rotz(ROL)*roty(PIT)*rotx(-YAW);
	tmp(Range(0,3), Range(0,3)).copyTo(R);

	Rodrigues(R, r);

	printf("3x3 rotation matrix\n");
	MATmtxdisp(R);
	printf("3x1 rotation vector\n");
	MATmtxdisp(r);
	getchar();
}


void C170_projection_test()
{	
	PBVS pbvs; 

	Mat P = Mat::zeros(3,1,CV_64F);
	// 1st test
	P.at<double>(0) = 0.095;
	P.at<double>(1) = -0.065;
	P.at<double>(2) = 0.3;
	Mat p;
	pbvs.C170Project(P,p);
	MATmtxdisp(p);

	// 2nd test
	P.at<double>(0) = 0.053;
	P.at<double>(1) = 0.12;
	P.at<double>(2) = 0.453;
	pbvs.C170Project(P,p);
	MATmtxdisp(p);

	// 3rd test
	P.at<double>(0) = -0.053;
	P.at<double>(1) = 0.12;
	P.at<double>(2) = 0.453;
	pbvs.C170Project(P,p);
	MATmtxdisp(p);

	getchar();
}

int main(int argc, char** argv)
{

	//RodriguesTest();

	C170_projection_test();
	
    return 0;
}